/***************************************************************************
 *   Copyright (C) 2008 by Isidoro Ferrante   *
 *   ferrante@isidoro.df.unipi.it   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include "bandpassfilter.hpp"
#include <iostream>
#include <stdexcept>
#include <math.h>
#include <sstream>

#ifndef M_PIl
/** The constant Pi in high precision */
#define M_PIl 3.1415926535897932384626433832795029L
#endif


bandpassfilter::bandpassfilter(int N, double A, double fs, double fstop, double f0)
{
    Initialize_filter(N);
    Calculate_Chebichev(A, fs, fstop,f0);
}



bandpassfilter::~bandpassfilter()
{
    int m;

    delete[] MLut;

    for (m=0; m<Mnb2; m++) {
        delete[] Ma2[m];
        delete[] Mb2[m];
        delete[] Mw2[m];
    }
    delete[] Mzeros;
    delete[] Mpoles;
    delete[] Ma1;
    delete[] Mb1;
    delete[] Mw1;
    delete[] Ma2;
    delete[] Mb2;
    delete[] Mw2;
}

void bandpassfilter::apply(cvectorf *out, vectorf *in)
{
  unsigned int i;
  float val;

  if(out->v->size != in->v->size) {
    throw std::invalid_argument("input and output with different sizes");
  }

  for(i=0;i<in->v->size;i++) {
      val = in->get(i);
      gsl_vector_complex_float_set(out->v,i,this->operator()(val));
  }
}

void bandpassfilter::Initialize_filter(int N)
{
    int m,i;
    MN=N;
    // calculates internal filter parameters

    // The number of second order blocks
    Mnb2= N/2;

    // The number of first order blocks
    Mnb1= N % 2;

    // allocate space for filter parameter and internal variables

    try {

        Mzeros= new cdouble[N];
        Mpoles= new cdouble[N];
        Ma1 = new double[3];
        Mb1 = new double[3];
        Mw1 = new cdouble[3];
        Ma2 = new doubleP[Mnb2];
        Mb2 = new doubleP[Mnb2];
        Mw2 = new cdoubleP[Mnb2];
    } catch (std::bad_alloc e) {
        std::cerr << "BAD ALLOCATION" << std::endl;
        throw(e);
    }

    //  per ogni riga, alloco gli elementi della riga
    for (m=0; m<Mnb2; m++) {
        try {
            Ma2[m] = new double[3];
            Mb2[m] = new double[3];
            Mw2[m] = new cdouble[3];
        } catch (std::bad_alloc e) {
            std::cerr << "BAD ALLOCATION" << std::endl;
            throw(e);
        }
    }

    for (m=0; m<Mnb2; m++) {
        // Initialize internal register
        for (i=0; i<3; i++) {
            GSL_SET_COMPLEX(Mw2[m]+i,0.0,0.0);
        }
    }
    GSL_SET_COMPLEX(Mw1,0.0,0.0);
    GSL_SET_COMPLEX(Mw1+1,0.0,0.0);
    Mindex=0;
}


void bandpassfilter::Calculate_Chebichev(double A, double fs, double fstop, double f0)
{
    int m;
    Mfs=fs;
    Mfstop=fstop;
    MA=A;
    Mos=2*M_PIl*fstop/fs;
    Mf0 = f0;

    Cheby2(MN, MA, Mos, Mzeros, Mpoles);

    for (m=0; m<Mnb2; m++) {
        zero_pole_to_coefficients(Mzeros[m],Mpoles[m], Ma2[m], Mb2[m]);
        // Initialize internal register
        if (Mnb1==1) {
            zero_pole_to_coefficients1(Mzeros[(MN-1)/2],Mpoles[(MN-1)/2], Ma1, Mb1);
        }
    }
    Calculate_Lut(Mf0);
}
void bandpassfilter::Calculate_Lut(double f0)
{
    double fn,df,deltaf;
    int k,T,np;
    fn=f0/Mfs;
    df=0.01*Mfstop/Mfs;
    np=1;
    T=(int)round(1./fn);
    deltaf=fn-1./T;
    deltaf=fabs(deltaf);

    while (deltaf>df) {
        np++;
        T=(int) round(np/fn);
        deltaf=fn-(float) np/ (float) T;
        deltaf=fabs(deltaf);
    }
    MLutLen=T;
    Mf0=Mfs*np/T;
    try {
      MLut= new cdouble[MLutLen];
    } catch( std::bad_alloc e) {
      std::cerr << "BAD ALLOC 3." << std::endl;
      throw(e);
    }
    for (k=0; k<MLutLen; k++) {
      gsl_complex tmp;
      GSL_SET_COMPLEX(&tmp,0.0,-2.0*M_PIl*k*np/T);
      MLut[k]=gsl_complex_exp(tmp);
    }
}


cfloat bandpassfilter::operator()(float x)
{
    cdouble y;
    cfloat yf;
    int m;
    /*index=fmod(filt->niter,filt->LutLen);*/
    y= gsl_complex_mul_real(MLut[Mindex],x);

    for (m=0; m<Mnb2; m++) {
        y = double_order_filt(y, Ma2[m] , Mb2[m], Mw2[m]);
    }
    if (Mnb1==1) {
        y = first_order_filt(y, Ma1 , Mb1, Mw1);
    }

    y=gsl_complex_mul(y,gsl_complex_conjugate(MLut[Mindex]));
    GSL_SET_COMPLEX(&yf,(float)GSL_REAL(y),(float)GSL_IMAG(y));

    Mindex++;
    Mindex=fmod(Mindex,MLutLen);

    return yf;
}

int bandpassfilter::Cheby2(int N, double A, double os, cdouble  *zeros, cdouble  *poles)
{
    /*
    Computes zeros and poles of a Chebychev filter type 2 given:
    The filter order N
    The attenuation in the stopband A (in decibel)
    The stopband edge ws
    */
    int m;
    double ws,thetam, wzm;
    double epsi, beta;
    cdouble iwpm,wpm;

    /*
    ws is the stopband edge in w
    epsi is an auxiliary variable
    */
    ws=tan(os/2);
    epsi=1/sqrt(pow(10.,(A*0.1))-1);
    for (m=0; m<N; m++) {
        thetam=M_PIl*(2*m+1)/(2*N);
        /*
        first compute the zeros
        */

        wzm=ws/cos(thetam);
        gsl_complex num,den;
        GSL_SET_COMPLEX(&num,1.0,wzm);
        GSL_SET_COMPLEX(&den,1.0,-wzm);
        zeros[m]=gsl_complex_div(num,den);

        /*
        printf("%i  %g   %g \n",m,creal(zeros[m]),cimag(zeros[m]));
        */

        /*
        Then compute the poles
        */
        beta=asinh(1/epsi)/N;
        GSL_SET_COMPLEX(&iwpm,cos(thetam)*cosh(beta),-sin(thetam)*sinh(beta));
        wpm=gsl_complex_mul_real(gsl_complex_inverse(iwpm),ws);
        GSL_SET_COMPLEX(&num,1.0-GSL_IMAG(wpm),GSL_REAL(wpm));
        GSL_SET_COMPLEX(&den,1.0+GSL_IMAG(wpm),-GSL_REAL(wpm));
        poles[m]=gsl_complex_div(num,den);
        /*printf("%i  %g   %g \n",m,creal(poles[m]),cimag(poles[m]));*/
    }

    return 0;
}


void bandpassfilter::zero_pole_to_coefficients(cdouble  zero, cdouble  pole, double *a, double  *b)
{
    double norm;
    int k;

    a[0]=1;
    a[1]=-2.0*GSL_REAL(pole);
    a[2]=gsl_complex_abs2(pole);
    b[0]=1;
    b[1]=-2.0*GSL_REAL(zero);
    b[2]=gsl_complex_abs2(zero);
    norm=(a[0]+a[1]+a[2])/(b[0]+b[1]+b[2]);

    for (k=0; k<3; k++) {
        b[k]=b[k]*norm;
    }
    /*
    printf("%g  %g   %g \n",a[0],a[1],a[2]);
    printf("%g  %g   %g \n",b[0],b[1],b[2]);
    printf("norma %g\n",norm);
    */
}


void bandpassfilter::zero_pole_to_coefficients1(cdouble  zero, cdouble  pole, double *a, double  *b)
{
    double norm;
    int k;

    a[0]=1.0;
    a[1]=-GSL_REAL(pole);
    b[0]=1.0;
    b[1]=-GSL_REAL(zero);
    norm=(a[0]+a[1])/(b[0]+b[1]);

    for (k=0; k<2; k++) {
        b[k]=b[k]*norm;
    }
    /*
    printf("%g  %g   \n",a[0],a[1]);
    printf("%g  %g   \n",b[0],b[1]);
    printf("norma %g\n",norm);
    */
}

cdouble bandpassfilter::double_order_filt(cdouble  x, double *a, double *b, cdouble  *w)
{
    cdouble  y;
    /* shifts the internal register by one sample */

    w[2]=w[1];
    w[1]=w[0];
    /*calculates the actual value of the internal register */

    w[0]= gsl_complex_sub(x,
                          gsl_complex_add(
                                            gsl_complex_mul_real(w[1],a[1]),
                                            gsl_complex_mul_real(w[2],a[2])
                                          )
                          );

    /*calculates the actual value of the filtered signal */
    y=gsl_complex_add(
      gsl_complex_mul_real(w[0],b[0]),
      gsl_complex_add(
                      gsl_complex_mul_real(w[1],b[1]),
                      gsl_complex_mul_real(w[2],b[2])
                      )
    );
    return y;
}

cdouble bandpassfilter::first_order_filt(cdouble  x, double *a, double *b, cdouble  *w)
{
    cdouble  y;
    /* shifts the internal register by one sample */

    w[1]=w[0];
    /*calculates the actual value of the internal register */

    w[0]=gsl_complex_sub(
                         x,
                         gsl_complex_mul_real(w[1],a[1])
                         );

    /*calculates the actual value of the filtered signal */
    y=gsl_complex_add(
                      gsl_complex_mul_real(w[0],b[0]),
                      gsl_complex_mul_real(w[1],b[1])
                      );
    return y;
}


